#include "CurrentSenser.h"
#include <stdio.h>

#include "MotorDriver.h"
#include "MT6701.h"
#include "FOC_transform.h"
#include "PID.h"

uint16_t adc_raw_buff[ADC_BUF_NUM][4] = {0};

uint8_t CurrentSenser_Init(void)
{
    wk_adc1_init();
    return 0;
}
uint8_t CurrentSenser_get_ADC(uint8_t phare, uint16_t *adc_raw)
{
    uint32_t temp = 0;
    uint16_t i;
    for (i = 0; i < ADC_BUF_NUM; i++)
    {
        temp += adc_raw_buff[i][phare];
    }
    *adc_raw = temp / ADC_BUF_NUM;

    return 0;
}
uint8_t CurrentSenser_get_current(uint8_t phare, uint16_t *adc_raw, float *current)
{
    float current_temp;
    CurrentSenser_get_ADC(phare, adc_raw);
    current_temp = ((float)*adc_raw - 2048.0f) / 4096.0f * 5.0f;
    // if(current_temp < -1.0f)
    // {
    //     current_temp = -1.0f;
    // }
    // else if(current_temp > 1.0f)
    // {
    //     current_temp = 1.0f;
    // }
    *current = current_temp;
    // *current = ((((float)*adc_raw - 2048) / CURRENT_SENSER_ADC_MAX * CURRENT_SENSER_ADC_REF)) / CURRENT_SENSER_GAIN / CURRENT_SENSER_R;
    return 0;
}

void it_get_adc(void)
{
    // static uint8_t led = 0;
    // if(led)
    // {
    //     gpio_bits_set(GPIOC, GPIO_PINS_13);
    //     led = 0;
    // }
    // else
    // {
    //     gpio_bits_reset(GPIOC, GPIO_PINS_13);
    //     led = 1;
    // }
    // printf("finish it_adc\n");
    // adc_flag_clear(ADC1, ADC_CCE_FLAG);
}

void CurrentSenser_calibrate(void)
{
    float angle;

    MT6701_init();         // 初始化 MT6701 传感器
    FOC_Calibrate_Angle(); // 校准角度
    FOC_elec_Angle(NULL,&angle); // 获取电角度
    // printf("a: %f\n", current);
}
